#include "kinectsensor.h"
#include <QDebug>


// Safe release for interfaces
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
    if (pInterfaceToRelease != NULL)
    {
        pInterfaceToRelease->Release();
        pInterfaceToRelease = NULL;
    }
}

//KinectSensor::KinectSensor(QObject *parent) : QObject(parent)
//{

//}
void KinectSensor::updatebody(){

    if (!m_pBodyFrameReader)
    {
        return;
    }

    IBodyFrame* pBodyFrame = NULL;

    HRESULT hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame);

    if (SUCCEEDED(hr))
    {
        INT64 nTime = 0;

        hr = pBodyFrame->get_RelativeTime(&nTime);

        //body position
        IBody* ppBodies[BODY_COUNT]={0};

        if (SUCCEEDED(hr))
        {
            hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);
        }

        if (SUCCEEDED(hr))
        {
//            ProcessBody(nTime, BODY_COUNT, ppBodies);
                getCurrentBody(ppBodies);

//            qDebug()<<"updating";
                 getBodyJoint();
                 mapJointsToXYCoord();

                 currentPBody->get_HandLeftState(&leftHandState);
                 currentPBody->get_HandRightState(&rightHandState);
        }

        for (int i = 0; i < _countof(ppBodies); ++i)
        {
            SafeRelease(ppBodies[i]);
        }
    }

    SafeRelease(pBodyFrame);

}


HRESULT KinectSensor::InitializeDefaultSensor(){
    HRESULT hr;
    hr = GetDefaultKinectSensor(&m_pKinectSensor);
    if (FAILED(hr))
    {
        return hr;
    }

    if (m_pKinectSensor)
    {
        // Initialize the Kinect and get coordinate mapper and the body reader
        IBodyFrameSource* pBodyFrameSource = NULL;

        hr = m_pKinectSensor->Open();

        if (SUCCEEDED(hr))
        {
            hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
        }

        if (SUCCEEDED(hr))
        {
            hr = m_pKinectSensor->get_BodyFrameSource(&pBodyFrameSource);
        }

        if (SUCCEEDED(hr))
        {
            hr = pBodyFrameSource->OpenReader(&m_pBodyFrameReader);
        }

        SafeRelease(pBodyFrameSource);
    }

    if (!m_pKinectSensor || FAILED(hr))
    {
//        SetStatusMessage(L"No ready Kinect found!", 10000, true);
        qDebug()<<"No ready Kinect found!";
        return E_FAIL;
    }





    return hr;

}

//void KinectSensor::ProcessBody(INT64 nTime, int nBodyCount, IBody** ppBodies){
//    if (m_hWnd){
//        HRESULT hr = EnsureDirect2DResources();

//        if (SUCCEEDED(hr) && m_pRenderTarget && m_pCoordinateMapper)
//        {
//            m_pRenderTarget->BeginDraw();
//            m_pRenderTarget->Clear();

//            RECT rct;
//            GetClientRect(GetDlgItem(m_hWnd, IDC_VIDEOVIEW), &rct);
//            int width = rct.right;
//            int height = rct.bottom;

//            for (int i = 0; i < nBodyCount; ++i)
//            {
//                IBody* pBody = ppBodies[i];
//                if (pBody)
//                {
//                    BOOLEAN bTracked = false;
//                    hr = pBody->get_IsTracked(&bTracked);

//                    if (SUCCEEDED(hr) && bTracked)
//                    {
//                        Joint joints[JointType_Count];
//                        D2D1_POINT_2F jointPoints[JointType_Count];
//                        HandState leftHandState = HandState_Unknown;
//                        HandState rightHandState = HandState_Unknown;

//                        pBody->get_HandLeftState(&leftHandState);
//                        pBody->get_HandRightState(&rightHandState);

//                        hr = pBody->GetJoints(_countof(joints), joints);
//                        if (SUCCEEDED(hr))
//                        {
//                            for (int j = 0; j < _countof(joints); ++j)
//                            {
//                                jointPoints[j] = BodyToScreen(joints[j].Position, width, height);
//                            }

//                            DrawBody(joints, jointPoints);

//                            DrawHand(leftHandState, jointPoints[JointType_HandLeft]);
//                            DrawHand(rightHandState, jointPoints[JointType_HandRight]);
//                        }
//                    }
//                }
//            }

//            hr = m_pRenderTarget->EndDraw();

//            // Device lost, need to recreate the render target
//            // We'll dispose it now and retry drawing
//            if (D2DERR_RECREATE_TARGET == hr)
//            {
//                hr = S_OK;
//                DiscardDirect2DResources();
//            }
//        }

//        if (!m_nStartTime)
//        {
//            m_nStartTime = nTime;
//        }

//        double fps = 0.0;

//        LARGE_INTEGER qpcNow = {0};
//        if (m_fFreq)
//        {
//            if (QueryPerformanceCounter(&qpcNow))
//            {
//                if (m_nLastCounter)
//                {
//                    m_nFramesSinceUpdate++;
//                    fps = m_fFreq * m_nFramesSinceUpdate / double(qpcNow.QuadPart - m_nLastCounter);
//                }
//            }
//        }

//        WCHAR szStatusMessage[64];
//        StringCchPrintf(szStatusMessage, _countof(szStatusMessage), L" FPS = %0.2f    Time = %I64d", fps, (nTime - m_nStartTime));

//        if (SetStatusMessage(szStatusMessage, 1000, false))
//        {
//            m_nLastCounter = qpcNow.QuadPart;
//            m_nFramesSinceUpdate = 0;
//        }
//    }

//}

bool KinectSensor::getCurrentBody(IBody **ppbodies){
    for(int a=0;a<BODY_COUNT;a++){

        currentPBody=ppbodies[a];
        if(currentPBody){
            HRESULT hr;
            BOOLEAN btracked;
            hr=currentPBody->get_IsTracked(&btracked);
            if(SUCCEEDED(hr)&&btracked){
//                qDebug()<<"succeed!";
                return true;
            }
        }
    }
    return false;
}

void KinectSensor::getBodyJoint(){
    if(currentPBody){
        HRESULT hr;
        Joint joints[JointType_Count];
       hr=currentPBody->GetJoints(_countof(joints), joints);
       if(SUCCEEDED(hr)){
           leftHandJoint=joints[JointType_HandLeft];
           rightHandJoint=joints[JointType_HandRight];

           topRightJoint=joints[JointType_ShoulderRight];
           bottomRightJoint=joints[JointType_HipRight];
           topLeftJoint=joints[JointType_ShoulderLeft];
           headJoint=joints[JointType_Head];
           neckJoint=joints[JointType_Neck];
       }
    }
}

void KinectSensor::mapJointsToXYCoord(){
    m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
    if(m_pCoordinateMapper){
        m_pCoordinateMapper->MapCameraPointToColorSpace(leftHandJoint.Position,&leftHandPoint);
        m_pCoordinateMapper->MapCameraPointToColorSpace(rightHandJoint.Position,&rightHandPoint);
    }
}

QString KinectSensor::getLeftHandState(){

    if(leftHandState==HandState_Unknown){
        return "HandState_Unknown";
    }
    else if(leftHandState==HandState_NotTracked){
        return "HandState_NotTracked";
    }
    else if(leftHandState==HandState_Open){
        return "HandState_Open";
    }
    else if(leftHandState==HandState_Closed){
        return "HandState_Closed";
    }
    else if(leftHandState==HandState_Lasso){
        return "HandState_Lasso";
    }
    else {
        return "I don't know hell";
    }


}

QString KinectSensor::getRightHandState(){

    if(rightHandState==HandState_Unknown){
        return "HandState_Unknown";
    }
    else if(rightHandState==HandState_NotTracked){
        return "HandState_NotTracked";
    }
    else if(rightHandState==HandState_Open){
        return "HandState_Open";
    }
    else if(rightHandState==HandState_Closed){
        return "HandState_Closed";
    }
    else if(rightHandState==HandState_Lasso){
        return "HandState_Lasso";
    }
    else {
        return "I don't know hell";
    }
}
